﻿//#include "stdafx.h"
#include <iostream>
#include <string>
#include <stdio.h>
//
#include "opencv2/opencv.hpp"
#include "PylonGrabber.h"


using namespace std;
using namespace cv;


const unsigned int cameraID = 0;


int main(int argc, char** argv) {

	/*printf("No arg %d\n", argc);

	for (int i=0; i<argc; i++)
	{
		printf("arg[%d] is %s\n", i,argv[i]);
	}
	getchar();*/

	//define chess-board variables
	int iNumBoards = 10; //how many successful boards to collect
	int iNumCornersHorizontal = 8;//7  Num of corner = Num of cell + 1
	int iNumCornersVertical = 6;//5
	//get input of chess-board info from users
	double disConersHori=7.4; // 18.75; mm
	double disConersVer=7.4; // 18.75; mm

	//setup more variables
	int iNumSquares = iNumCornersHorizontal * iNumCornersVertical;
	Size sBoardSize = Size(iNumCornersHorizontal, iNumCornersVertical);


	// NORMAL WEB CAMERA
	//setup video capture
	/*VideoCapture capture = VideoCapture(cameraID);
	if(!capture.isOpened()) {
		printf("camera %d open failed \n", cameraID);
		return -1;
	}*/

	// PYLON CAMERA
	PylonGrabber pg; 

	/*
	 * important: setup object points and image points
	 */
	vector< vector<Point2f> > imagePoints;
	vector< vector<Point3f> > objectPoints;
	//setup corners
	vector<Point2f> corners;
	vector<Point3f> obj;

	int successes = 0;

	//setup image
	Mat image;
	Mat gray_image;
	//capture >> image;

	//setup view win
	namedWindow("win1",1);
	namedWindow("win2", 1);
	
	//setup init obj points
	for(int j = 0; j < iNumSquares; j++){
		obj.push_back(Point3f(j/iNumCornersHorizontal*disConersHori,
				j%iNumCornersHorizontal*disConersVer,
				0.0f));
		//print points
		
		printf("Object[%d]->Point3f(%d, %d, 0)\n",
				j,
				j/iNumCornersHorizontal,
				j%iNumCornersHorizontal);
	}

	//main loop - checking chessboards
	while(successes < iNumBoards){
		// capture >> image;
		pg.grabFrame(image);

		// check captured image
		//imshow("captured",image);
		//waitKey(0);

		//cvtColor(image, gray_image, CV_BGR2GRAY, 0);
		image.copyTo(gray_image);

		bool found = findChessboardCorners(image,
				sBoardSize,
				corners,
				CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
		if(found){
			cornerSubPix(gray_image,
					corners,
					Size(11, 11),
					Size(-1, -1),
					TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)
					);
			drawChessboardCorners(image, sBoardSize, corners, found);
		}
		
		//keyboard event listener
		int key = waitKey(10);
	
		if(key == 32 && found){ //32 is the spacebar key
			bitwise_not(image, image); //to blink
			
			successes ++;
			//
			imagePoints.push_back(corners);
			objectPoints.push_back(obj);
			printf("Got Chessboard Snap %d/%d \n",
					successes,
					iNumBoards);
			//
			if(successes >= iNumBoards){
				printf("Captured ALL successful snaps, exiting... \n");
				break;
			}
			
		}
		
		//show img
		imshow("win1", image);
		imshow("win2", gray_image);
	}
	
	//setup camera matrix and distcoeffic
	Mat intrinsic = Mat(3, 3, CV_32FC1);
	Mat distCoeffs;
	vector<Mat> rvecs;
	vector<Mat> tvecs;
	
	//
	intrinsic.ptr<float>(0)[0] = 1;
	intrinsic.ptr<float>(1)[1] = 1;

	//finds intrinsic and extrinsic camera parameters
	printf("starting calibrating camera... \n");
	calibrateCamera(objectPoints,
			imagePoints,
			image.size(),
			intrinsic,
			distCoeffs,
			rvecs,
			tvecs,
			0,
			TermCriteria( TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
			
		//save the params into file
		printf("saving camera matrix file \n");
		FileStorage camMatrix("intrinsic.yaml", FileStorage::WRITE);
		camMatrix << "imageContent" << intrinsic;
		camMatrix.release();
		printf("saving coefficicnet params \n");
		FileStorage coeffic("coeffic.yaml", FileStorage::WRITE);
		coeffic << "imageContent" << distCoeffs;
		
		//setup img to show undist image
		Mat imageUndist;
		namedWindow("undist", 1);
		
		
		double a00=intrinsic.at<double>(0,0);
		double a01=intrinsic.at<double>(0,1);
		double a02=intrinsic.at<double>(0,2);
		double a10=intrinsic.at<double>(1,0);
		double a11=intrinsic.at<double>(1,1);
		double a12=intrinsic.at<double>(1,2);
		double a20=intrinsic.at<double>(2,0);
		double a21=intrinsic.at<double>(2,1);
		double a22=intrinsic.at<double>(2,2);
	    
		printf("a00=%f\n",a00);
		printf("a01=%f\n",a01);
		printf("a02=%f\n",a02);
		printf("a10=%f\n",a10);
		printf("a11=%f\n",a11);
		printf("a12=%f\n",a12);
		printf("a20=%f\n",a20);
		printf("a21=%f\n",a21);
		printf("a22=%f\n",a22);

		//double aaaa=distCoeffs.at<double>(0,0);
		//printf("da00=%f\n",aaaa);
		double da00=distCoeffs.at<double>(0,0);
		double da01=distCoeffs.at<double>(0,1);
		double da02=distCoeffs.at<double>(0,2);
		double da03=distCoeffs.at<double>(0,3);
		double da04=distCoeffs.at<double>(0,4);
		
		printf("da00=%f\n",da00);
		printf("da01=%f\n",da01);
		printf("da02=%f\n",da02);
		printf("da10=%f\n",da03);
		printf("da11=%f\n",da04);
		
		



		//solvePnP(objectPointMat,imgObPointMat,matIntrin,matDis,RotVector,TransVector,0,0);
		////cvFindExtrinsicCameraParams2(obPointMat,imgObPoint,sl_calib->cam_intrinsic,sl_calib->cam_distortion,rotateR,TranslateT);
		//Rodrigues(RotVector,RotMatrix,JacobianM);
		IplImage * imgframe=NULL;
		// CvCapture * capture2=NULL;
		//loop to show undist image
		while(1){

			//capture2=cvCaptureFromCAM(0);
			//imgframe=cvQueryFrame(capture2);
			pg.grabFrame(Mat(imgframe));

			cvShowImage("img",imgframe);
			int c=cvWaitKey(10);
			if (c=='t')
			{
				IplImage * imgChess=cvCreateImage(cvSize(640,480),8,3);
				IplImage * imgDraw=cvCreateImage(cvSize(640,480),8,3);
				cvCopy(imgframe,imgChess);
				cvCopy(imgframe,imgDraw);
				int height_Chessboard=6; //短邊
				int width_Chessboard=8;  //長邊
				int heightxwidth = height_Chessboard*width_Chessboard;
				double obPointOrder[48][3]={0};   // this one needs dynamic memory allocation, it is ugly to define manually like this => next time use vector
				double imgi[48]={0};
				double imgj[48]={0};
				
				cvShowImage("original ",imgChess);	
				IplImage *capimg_gray=cvCreateImage(cvSize(640,480),8,1);
				cvCvtColor(imgChess,capimg_gray,CV_BGR2GRAY);
				Size patternsize(width_Chessboard,height_Chessboard); //interior number of corners
				Mat imgShow=cvCloneImage(imgChess);
				Mat gray =cvCloneImage(capimg_gray); //source image
				vector<Point2f> corners; //this will be filled by the detected corners
				bool patternfound = findChessboardCorners(gray, patternsize, corners,
					CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE
					+ CALIB_CB_FAST_CHECK);
				
				double chessX=0;
				double chessY=0;
				double chessZ=0;
				

				for (int wi=0;wi<height_Chessboard;wi++) ///6
				{
					for (int wj=0;wj<width_Chessboard;wj++) ///8
					{

						chessX=height_Chessboard*wi;
						chessY=width_Chessboard*wj;
						obPointOrder[wi*width_Chessboard+wj][0]=chessX;
						obPointOrder[wi*width_Chessboard+wj][1]=chessY;
						obPointOrder[wi*width_Chessboard+wj][2]=chessZ;
					}
				}
				vector<Point3f> obPoint;
				vector<int>::size_type ssi=1; 
				int sss=0;
				for(ssi;ssi<=width_Chessboard*height_Chessboard;++ssi)
				{
					Point3f points3D; 
					points3D.x=obPointOrder[sss][0];
					points3D.y=obPointOrder[sss][1];
					points3D.z=obPointOrder[sss][2];
					obPoint.push_back(points3D);
					sss=sss+1;
				}
				Mat ob_P=Mat(3,heightxwidth,CV_64FC1);    // 48 

				for(int ci=0; ci<heightxwidth; ci++)     // 48 
				{

					ob_P.at<double>(3*ci+0) = (double)(obPointOrder[ci][0]);
					ob_P.at<double>(3*ci+1) = (double)(obPointOrder[ci][1]);
					ob_P.at<double>(3*ci+2) = (double)(obPointOrder[ci][2]);
				}
				Mat objectPointMat=Mat(width_Chessboard*height_Chessboard,3,CV_64FC1);
				objectPointMat=Mat(obPoint);
				Mat imgObPointMat=Mat(width_Chessboard*height_Chessboard,2,CV_32FC1);
				imgObPointMat=Mat(corners);
				Mat RotVector,TransVector,RotMatrix,JacobianM;
				Mat CamCornerCoordinate=Mat(3,width_Chessboard*height_Chessboard,CV_64FC1);
				if(patternfound)
				{	
					cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1),
						TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
					drawChessboardCorners(imgShow, patternsize, Mat(corners), patternfound);
					imgChess=cvCloneImage(&(IplImage)imgShow);  
					cvShowImage("test Draw Corner",imgChess);
					/*Mat matIntrin(sl_calib->cam_intrinsic);
					Mat matDis(sl_calib->cam_distortion);*/
					/*cout<<"cam_intrin "<<matIntrin<<endl;
					cout<<"cam_Dis "<<matDis<<endl;*/
					solvePnP(objectPointMat,imgObPointMat,intrinsic,distCoeffs,RotVector,TransVector,0,0);
					//cvFindExtrinsicCameraParams2(obPointMat,imgObPoint,sl_calib->cam_intrinsic,sl_calib->cam_distortion,rotateR,TranslateT);
					Rodrigues(RotVector,RotMatrix,JacobianM);
					Mat TransMatrix=Mat(3,width_Chessboard*height_Chessboard,CV_64FC1);
					for (int ti=0; ti < heightxwidth; ti++)
					{

						TransMatrix.at<double>(3*ti+0)=TransVector.at<double>(0);
						TransMatrix.at<double>(3*ti+1)=TransVector.at<double>(1);
						TransMatrix.at<double>(3*ti+2)=TransVector.at<double>(2);

					}
					cout<<"the rotate Vector is "<<RotVector<<endl;
					cout<<"rotate Matrix is "<<RotMatrix<<endl;
					cout<<"Translate Matrix is "<<TransVector<<endl;
				}
			}

			cvReleaseImage(&imgframe);
			//capture >> image;
			//undistort(image, imageUndist, intrinsic, distCoeffs);
			//imshow("undist", imageUndist);
			//int file_idx = 0;
			//
			//char filename[256];
			//char fileidx[10];
			//IplImage * Catchframe=cvCloneImage(&(IplImage)imageUndist);   

			//int key = waitKey(1);
			//if (key=='s'|| key=='S')
			//{

			//	sprintf( fileidx, "%03d", file_idx++ );
			//	strcpy( filename, "bb" );
			//	strcat( filename, fileidx );
			//	strcat( filename, ".jpg" );

			//	cvSaveImage( filename, Catchframe );
			//	printf("Image file %s saved.\n", filename);
			//}
			//if(key == 27) break; //keyboard event
		}
		
	//release resource
	//capture.release();
	return 0;
}